@ MODE          IRQ ENABLED     IRQ DISABLED
@ ===========================================
@ Supervisor    0x13            0xd3    	
@ User          0x10            0xd0
@ System        0x1F            0xdf
@ IRQ           0x12            0xd2
	
	.text
	.align	2
	.global	EnableCache
	.type	EnableCache, %function
mask:
  .word 0x1000
  @.word 0x1004
EnableCache:
  stmfd	sp!, {r0, r1}
  mrc p15, 0, r0, c1, c0, 0   @ read c1
  @ and r0, r0, mask            @ clear bits to be cleared
  ldr r1, mask
  orr r0, r0, r1           @ set bits to be set
  mcr p15, 0, r0, c1, c0, 0   @ write c1 
  ldmfd	sp, {r0, r1}
	mov pc, lr
	
.global	KernelExitIRQ
@ void KernelExitIRQ( td *task, req *request )
KernelExitIRQ:
    stmdb  sp!, {r4-r12,lr}     @ push kernel state on kernel stack
    str r0, shared_td           @ set aside TD for KernelEnter()
    str r1, shared_req          @ set aside request for KernelEnter()
    ldr r0, [r0, #8]            @ get user spsr from TD
    bic r0, r0, #0x80           @ enable IRQ interrupts in user spsr
    stmdb sp!, {r0}             @ push user spsr on kernel stack
    msr cpsr, #0xdf           @ switch to system mode
    ldr r0, shared_td
    ldr sp, [r0, #4]            @ get user sp from TD
    ldmia  sp!, {r0}
    str r0, shared_lr
    ldmia  sp!, {r0-r12,lr}     @ pop user registers from user stack
    str r0, shared_r0           @ set aside r0 - we use it as scratch space :)
    msr cpsr, #0xd3           @ switch to supervisor mode
    ldr r0, shared_lr
    mov lr, r0                  @ restore lr
    ldmia sp!, {r0}             @ pop user spsr from kernel stack
    msr spsr, r0                @ install spsr in supervisor for movs/subs
    ldr r0, shared_r0           @ restore r0
    subs pc, lr, #4             @ jump to user mode after IRQ triggered

.global	KernelExitSWI
@ void KernelExitSWI( td *task, req *request )
KernelExitSWI:
    stmdb  sp!, {r4-r12,lr}     @ push kernel state on kernel stack
    str r0, shared_td           @ set aside TD for KernelEnter()
    str r1, shared_req          @ set aside request for KernelEnter()
    ldr r0, [r0, #8]            @ get user spsr from TD
    bic r0, r0, #0x80           @ enable IRQ interrupts in user spsr
    stmdb sp!, {r0}             @ push user spsr on kernel stack
    msr cpsr, #0xdf           @ switch to system mode
    ldr r0, shared_td
    ldr sp, [r0, #4]            @ get user sp from TD
    ldmia  sp!, {r0}
    str r0, shared_lr
    ldmia  sp!, {r0-r12,lr}     @ pop user registers from user stack
    msr cpsr, #0xd3           @ switch to supervisor mode
    ldr r0, shared_lr
    mov lr, r0                  @ restore lr
    ldmia sp!, {r0}             @ pop user spsr from kernel stack
    msr spsr, r0                @ install spsr in supervisor for movs/subs
    ldr r0, shared_td
    ldr r0, [r0, #12]           @ load return value into r0
    movs pc, lr                 @ jump to user mode after swi

.global KernelEnterIRQ
KernelEnterIRQ:
    str lr, shared_lr           @ set aside lr
    msr cpsr, #0xdf           @ switch to system mode
    stmdb sp!, {r0-r12, lr}     @ push user registers on user stack
    ldr r0, shared_lr           @ restore lr (instruction after swi)
    stmdb sp!, {r0}
    ldr r0, shared_td
    mov r1, sp                  @ save user sp in TD
    str r1, [r0, #4]
    msr cpsr, #0xd2           @ switch to IRQ mode
    mrs r1, spsr                @ save user spsr in TD
    str r1, [r0, #8]
    msr cpsr, #0xd3           @ switch to supervisor mode
    ldr r0, shared_req          @ indicate IRQ by call number 0xff
    mov r1, #0xff
    str r1, [r0, #0]
    ldr r0, shared_td           @ set mode in TD - 1 is IRQ
    mov r1, #1
    str r1, [r0, #16]
    ldmia sp!, {r4-r12, lr}     @ pop kernel registers from kernel stack
    mov pc, lr                  @ jump to Activate() after KernelExit()

.global KernelEnterSWI
KernelEnterSWI:
    str lr, shared_lr           @ set aside lr
    msr cpsr, #0xdf           @ switch to system mode
    stmdb sp!, {r0-r12, lr}     @ push user registers on user stack
    ldr r0, shared_lr           @ restore lr (instruction after swi)
    stmdb sp!, {r0}
    ldr r0, shared_td
    mov r1, sp                  @ save user sp in TD
    str r1, [r0, #4]
    msr cpsr, #0xd3           @ switch to supervisor mode
    mrs r1, spsr                @ save user spsr in TD
    str r1, [r0, #8]
    ldr r1, shared_req
    ldr r0, [lr, #-4]           @ get system call params
    and r0, r0, #0xff
    str r0, [r1, #0]
    ldr r0, [fp, #-20]          @ get arg 1
    str r0, [r1, #4]
    ldr r0, [fp, #-24]          @ get arg 2
    str r0, [r1, #8]
    ldr r0, [fp, #-28]          @ get arg 3
    str r0, [r1, #12]
    ldr r0, [fp, #-32]          @ get arg 4
    str r0, [r1, #16]
    ldr r0, [ip, #0]            @ get arg 5
    str r0, [r1, #20]
    ldr r1, shared_td           @ set mode in TD - 0 is SWI
    mov r0, #0
    str r0, [r1, #16]
    ldmia sp!, {r4-r12, lr}     @ pop kernel registers from kernel stack
    mov pc, lr                  @ jump to Activate() after KernelExit()

@ global state used by the context switch
shared_td:
    .space 4
shared_req:
    .space 4
shared_lr:
    .space 4
shared_sp:
    .space 4
shared_spsr:
    .space 4
shared_r0:
    .space 4
